#pragma once
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/filters/voxel_grid.h>
#include <ArgParser.h>
#include <common.h>
#include <eigen3/Eigen/Dense>

namespace evaluation{
    class Evaluator
    {
    private:
        int plains_count = 0;	
	    double standard_deviation_sum = 0;
        double mean_dis_sum = 0;
        double cur_sd;
        int max_iteration;

        float search_step;
        float min_inliers_percent;
    
    public:
        void setArgs(ArgType& args)
        {
            max_iteration = args.dis_threshold*100;
            search_step = args.search_step;
            min_inliers_percent = args.min_inliers_percent;
        }

        float getMean(){ return mean_dis_sum / plains_count*100;}
        float getStandardDeviation(){ return (standard_deviation_sum / plains_count)*100; }
        
        void addPointCloud(PointCloud::Ptr cloud)
        {
            double in;  
			double dis_sum = 0, inliers_percent;
			double mean_dis = 0, dis_i;
			PointType pt;
			Eigen::VectorXf coef;
			for (int z = 0; z < max_iteration; z++)
			{
				in = search_step * (z + 1);  // distance threshold from 0.01 to 2.00(m)
		
				// fit the plain
				std::vector<int> inliers;
				pcl::SampleConsensusModelPlane<PointType>::Ptr model_p(new pcl::SampleConsensusModelPlane<PointType>(cloud));
				pcl::RandomSampleConsensus<PointType> ransac(model_p);
				ransac.setDistanceThreshold(in);
				ransac.computeModel();
				ransac.getInliers(inliers);
				inliers_percent = inliers.size() / cloud->points.size();
				//log_file << "dis_threshold = " << in << ", inliers_percent = " << inliers_percent << std::endl;
				if(inliers_percent < min_inliers_percent)
					continue;
				
				
				// log_file << "--------------------------------------------------------" << std::endl;
				// coefficients of the plain
				coef = Eigen::VectorXf::Zero(4, 1);
				ransac.getModelCoefficients(coef);
				double a = coef[0];
				double b = coef[1];
				double c = coef[2];
				double d = coef[3];
				double mo = sqrt(a*a + b*b + c*c);
				
				// caculate the sum of distance
				for (size_t i = 0; i <cloud->points.size(); i++){
					pt = cloud->points[i];
					dis_sum += fabs( a*pt.x+b*pt.y+c*pt.z+d )/mo;
				}
				// caculate mean distance
				mean_dis  = dis_sum / cloud->points.size();
				
				double deviation_square_n = 0;
				for(size_t i = 0; i< cloud->points.size(); i++)
				{
					pt = cloud->points[i];
					dis_i = fabs( a*pt.x+b*pt.y+c*pt.z+d )/mo;
					deviation_square_n += (dis_i - mean_dis) * (dis_i - mean_dis);
				}
				// caculate standard deviation of each plain
				mean_dis_sum += mean_dis;
				cur_sd = sqrt( deviation_square_n / (cloud->points.size() -1 ));
				
               
				standard_deviation_sum += cur_sd;			
				plains_count ++;
				break;
			}
        }
    };
}